/** \file    warp.cpp
  * \brief   Brief description.
  * \date    2001
  * \author  Fedor Chelnokov
  * \version $Header: /home/cscom1r4/cvsroot/cs/imglib/src/warp.cpp,v 1.1 2004/02/04 14:43:23 cscom1r4 Exp $
  */

#include "imglib/warp.h"
#include "imglib/bilinear.h"
#include "imglib/bitmap.h"

#pragma warning(disable: 4244)

namespace img {

struct ApprxData {
    //current point
    int x, y;
    //current approximation
    float nx, ny;
    //current value
    float fn_x, fn_y;

	void clear( const QuadraticForm &qf ) {
		x = y = nx = ny = 0;
		fn_x = qf.a0;
		fn_y = qf.b0;
	}
};

///finds preimage of one point on one frame back
///returns true on success
static bool transform( 
	ApprxData &a,			///< current approximation of quadratic form inversion
	const QuadraticForm &qf, 
	Point2f &backPlace,		///< place of point x,y on previous frame
	int x, int y )
{
    //make more precise iterations
    const float WORST_DIFF = 0.01f;
    const int MAX_ITER = 100;

    a.fn_x += a.x - x;
    a.fn_y += a.y - y;
    //current discrepancy
    float d = fabs(a.fn_x) + fabs(a.fn_y);
    a.x = x;
    a.y = y;

    //increment
    float dx, dy;
    //next approximation
    float n1_x, n1_y;
    //next value
    float fn1_x, fn1_y;
    //next discrepancy
    float d1;

    if( d > WORST_DIFF ) {
        unsigned iter;
        for( iter = 0;; ) {
            //make step according to newtonian linearization
            float c1 = qf.a1+1 + 2*qf.a3*a.nx + qf.a4*a.ny;
            float c2 = qf.a2 + qf.a4*a.nx + 2*qf.a5*a.ny;
            float c3 = qf.b1 + 2*qf.b3*a.nx + qf.b4*a.ny;
            float c4 = qf.b2+1 + qf.b4*a.nx + 2*qf.b4*a.ny;
            float det_power1 = 1. / (c1*c4 - c2*c3);

            dx = -det_power1 * ( c4*a.fn_x - c2*a.fn_y );
            dy = -det_power1 * ( -c3*a.fn_x + c1*a.fn_y );

            while(true) {
                if( ++iter == MAX_ITER ) {
					a.clear(qf);
					return false;
				}
                //try new position
                n1_x = a.nx + dx;
                n1_y = a.ny + dy;

                //f(n1)
                fn1_x = -x + qf.a0 + (qf.a1+1)*n1_x + qf.a2*n1_y + qf.a3*n1_x*n1_x + qf.a4*n1_y*n1_x + qf.a5*n1_y*n1_y;
                fn1_y = -y + qf.b0 + qf.b1*n1_x + (qf.b2+1)*n1_y + qf.b3*n1_x*n1_x + qf.b4*n1_y*n1_x + qf.b5*n1_y*n1_y;

                d1 = fabs(fn1_x) + fabs(fn1_y);
                if( d1 < d ) break;

                //try again with half step
                dx *= 0.5;
                dy *= 0.5;
            }

            //make next the current
            a.nx = n1_x;
            a.ny = n1_y;
            d = d1;
            a.fn_x = fn1_x;
            a.fn_y = fn1_y;

            //sufficiently near?
            if( d1 < WORST_DIFF ) break;
        }
    }
	backPlace.x = a.nx;
	backPlace.y = a.ny;
	return true;
}

void warpPreimage(PntArr2f &preimage, const QuadraticForm &qf, 
	float reservedPreimageValue, const int_point &ref) 
{
	ApprxData a;
	a.clear(qf);

	PntArr2f backPntPlaces( preimage.Width(), preimage.Height() );
	bitmap backPntFound( preimage.Width(), preimage.Height() );

    //snake
    int x, y;
    for( x = 0; x < preimage.Width(); ++x ) {
        if( x%2 == 0 ) {
            for( y = 0; y < preimage.Height(); ++y ) {
                backPntFound.at(x,y) = 
					transform(a, qf, backPntPlaces(x,y), 
						x-ref.x, y-ref.y);
				backPntPlaces(x,y).x += ref.x;
				backPntPlaces(x,y).y += ref.y;
            }
        } else {
            for( y = preimage.Height()-1; y >= 0; --y ) {
                backPntFound.at(x,y) = 
					transform(a, qf, backPntPlaces(x,y), 
						x-ref.x, y-ref.y);
				backPntPlaces(x,y).x += ref.x;
				backPntPlaces(x,y).y += ref.y;
            }
        }
    }

	bilinearMap( backPntPlaces, preimage, reservedPreimageValue );
    for( y = 0; y < preimage.Height(); ++y ) {
	    for( x = 0; x < preimage.Width(); ++x ) {
			if( !backPntFound.at(x,y) ) {
				preimage(x,y).x = reservedPreimageValue;
				preimage(x,y).y = reservedPreimageValue;
			}
		}
	}
}

} //namespace img
